#include <Second_road.h>

extern u8 Color[6];

extern u8 P1;
extern u8 P2;
extern u8 P3;
extern u8 P4;
extern u8 P5;
extern u8 P6;
extern u8 P7;
extern u8 P8;
extern u8 Scene_Cnt;
extern u8 Home_Cnt;

void Get_Home_1(void) //����վ��ƽ̨�� ���ε��Ҳ���Ҫתƽ̨ �泯����ƽ̨
{
	
	//����һ�� ��ת ׼����ʱ�����ٴ�
	
	while(d2)
	hunting_pid (2.5,50);
	while(d4)
	speed(18,18);
	stop();
	Delay_ms(50);
	while(d1)
	speed(35,-35);
	Front_Turn_Right();
	Delay_PID_ms(500,3,25);
	while(d2) hunting_pid(5, 25);
	
	//��ʱ�����ٴ�
	Delay_LPID_ms(1400, 5, 15);
	Beep(50,1);
	//��һ��
	while(d1) run(30);
	
	//��ʱ������ת
	speed(50, 25);
	Delay_ms(300);

	//�ص�����
	Delay_PID_ms(200, 3, 32);

	//��һ��ƽ̨  ����
	while(compassRead_Y() < 1900) hunting_pid(3,40);
	while(compassRead_Y() > 1850 && d1 && d2) hunting_pid(3,40);
	while(d3 && d4) speed(40,40);
	speed(50,50);
	Delay_ms(30);
	Home_Cnt ++ ;
	Show_Score();
}

void Get_Home_2(void) //����վ��ƽ̨�� ���ε��Ҳ���Ҫתƽ̨ �泯�ĺ�ƽ̨
{
	
	//����һ�� ��ת ׼����ʱ�����ٴ�
	Delay_PID_ms(200,2,80);
	Beep(50,1);
	while(d3)
	hunting_pid(2.6,40);
	while(d1)
	hunting_pid(2.6,30);
	stop();
	Delay_ms(100);
	while(d2)
	speed(-20,20);
	while(a4 < ac4 && a5 < ac5)
	speed(-20,20);
	Delay_PID_ms(500,3,25);
	while(d1) hunting_pid(5, 25);

	//��ʱ�����ٴ�
	Delay_LPID_ms(1400, 5, 15);
	Beep(50,1);

	//��һ��
	while(d1) run(30);

	//��ʱ������ת
	speed(50, 25);
	Delay_ms(300);

	//�ص�����
	Delay_PID_ms(200, 3, 32);

	//��һ��ƽ̨  ����
	while(compassRead_Y() < 1900) hunting_pid(3,40);
	while(compassRead_Y() > 1850 && d1 && d2) hunting_pid(3,40);
	while(d3 && d4) speed(40,40);
	speed(50,50);
	Delay_ms(30);
	Home_Cnt ++ ;
	Show_Score();
}	

void One_Green(void)  //һ�ű�ʶ��Ϊ��
{
	while(d0)
	stop();
	while(!d0)
	stop();	
	Part_1_34_L();	 

	//���ĺ�ƽ̨
	Delay_PID_ms(400, 5, 25);  //���ĺ�ƽ̨ǰ�Ľڵ�
	Delay_PID_ms(200, 2, 50);
		
	//��ɫʶ�𲿷ֳ���
	//��ת��1������ ��ɫ�Ƴ�ֱ������
	//������һ��
	Delay_PID_ms(650, 0.8, 80);
	Beep(50,1);	
	while(d1) hunting_pid(5, 25); 
	Delay_Turn_Left();

	
	//����һ�� ��ת�������ƽ̨
	
	Delay_PID_ms(300, 4.5, 25);
	Delay_PID_ms(200, 2, 50);  // �ȶ�����
	Delay_PID_ms(600, 1.2, 80);  // ������ʮ��·��ǰ
	Delay_PID_ms(100, 2.6, 60);  // �ȶ�����
	
	while(d1) hunting_pid(3, 25); 
	Delay_Turn_Left();
	
	//����������� 
	
	Delay_PID_ms(300,3,20);
	Beep(50,1);
	Delay_PID_ms(200,2,50);
	Beep(50,1);
	Delay_PID_ms(400,1.2,80);
	Beep(50,1);
	Part_3_78_L();			
	
	//���������� ��ת
	
	Front_Turn_Left();
	
	//��һС�� ��ת
	while(d1) hunting_pid(3, 30); 
	Delay_Turn_Left();
	
	//������ ��ת
	
	Delay_PID_ms(400,3,25);
	Delay_PID_ms(200,2,50);	
	Delay_PID_ms(800,1.2,80);
	Beep(50,1);
	while(d1) hunting_pid(1.2, 25); 
	Delay_Turn_Left();
	
	//������ ��ת 
	
	Delay_PID_ms(400,3,25);
	while(d1)	
	hunting_pid(2,50);
	while(d2)
	hunting_pid(0.8,80);
	while(d4)
	hunting_pid(0.8,80);
	while(d2)
	hunting_pid(0.8,80);
	while(d4)
	hunting_pid(0.8,80);
	while(d1)
	hunting_pid(3,40);
	while(d3)
	speed(18,18);
	stop();
	Delay_ms(50);
	Front_Turn_Left();
	
	//��һС�� ׼���ؼ�
	
	Delay_PID_ms(300,3,25);
	Delay_PID_ms(200,2,50);
	
	//�ؼ�
	
	Get_Home_1();
}

void Four_Green(void) //�ĺű�ʶ��Ϊ��
{
	while(d0)
	stop();
	while(!d0)
	stop();	
	Part_1_43_L();
	
	//��ɫʶ�𲿷ֳ���
	//��ɫ�Ƴ�ֱ������ ��ת�����ߡ��˺�֮������ɽ��ֱ������
	Delay_PID_ms(400,3,25);
	Delay_PID_ms(200,2,50);
	Delay_PID_ms(550, 1.2, 80);
	Beep(50,1);	
	while(d1)
	hunting_pid(2.6,60);
	Delay_PID_ms(200,2.6,60);
	while(d2) hunting_pid(3, 25); 
	
	Delay_Turn_Right();
	
	//����������� 
	Part_3_87_L();
	
	//���������� ��һ�� ��ת
	Delay_PID_ms(300,3,20);
	while(d2)
	hunting_pid(2.6,60);
	while(d4)            
	hunting_pid(3,30);
	while(d2)
	hunting_pid(3,30);	
	Delay_Turn_Right();
	
	//��ֱ������ ��ת
	Delay_PID_ms(400,3,25);
	Delay_PID_ms(200,2,50);	
	Delay_PID_ms(800,1.2,80);
	Beep(50,1);
	while(d2) hunting_pid(1.2, 30); 
	Delay_Turn_Right();
	
	//��ֱ������ ��ת 
	Delay_PID_ms(400,3,25);
	Delay_PID_ms(200,2,50);
	while(d4)
	hunting_pid(0.8,80);
	Delay_PID_ms(300,0.8,80);
	while(d2)
	hunting_pid(0.8,80);
	while(d4)
	hunting_pid(0.8,80);
	while(d2)
	hunting_pid(3,40);
	while(d4)
	speed(18,18);
	stop();
	Delay_ms(50);
	Front_Turn_Right();
	
	//׼���ؼ�
	Delay_PID_ms(300,3.5,25);
	Delay_PID_ms(200,2,50);
	Get_Home_2();
}

void Three_Green(void)  //���ű�ʶ��Ϊ�� S���ʶ��Ϊ��
{
	while(d0)
	stop();
	while(!d0)
	stop();	
	Part_1_34S_L();
	
	//�����������
	Part_3_78_L();
	
	//��ת ��һС�� ��ת
	Front_Turn_Right();
	Pass_Short_d2d4();
	while(d1)
	speed(35,-35);
	
	//����һ�� ��ת
	Delay_PID_ms(400, 3, 25);  // �ȶ�����	
	Delay_PID_ms(400, 0.8, 80);  // ����
	Delay_PID_ms(200, 0.8, 60);  // �ȶ�����		
	Pass_Short_d1d3();
	Front_Turn_Left();	
	
	//����һ�� ��ת
	Delay_PID_ms(300, 3, 25);  // �ȶ�����	
	while(d4)
	hunting_pid(1.2,80);
	Delay_PID_ms(200, 1.2, 80);  // ����
	Pass_Short_d2d4();
	Front_Turn_Right();	
	Delay_PID_ms(300,3.5,25);
	Delay_PID_ms(200,2,50);
	//׼���ؼ�
	Get_Home_2();
}


void Two_Green(void)    //���ű�ʶ��Ϊ�� 
{  
	while(d0)
	stop();
	while(!d0)
	stop();	
	Part_1_34_L();
	
	//���ĺ�ƽ̨
	Delay_PID_ms(400, 5, 25);  //���ĺ�ƽ̨ǰ�Ľڵ�
	Delay_PID_ms(200, 2, 50);
	
	//��ɫʶ�𲿷ֳ���
	//��ת��2������ ��ɫ�Ƴ�ֱ������
	
	Delay_PID_ms(550, 0.8, 80);
	Beep(50,1);	
	while(d1)
	hunting_pid(3,40);
	while(d3)
	speed(18,18);
	stop();
	Delay_ms(50);
	while(d2)
	speed(-30,30);
	Delay_PID_ms(300,3,20);
	Delay_PID_ms(200,2,50);
	
	//������ ��ת
	while(d3)
	hunting_pid(2.5,75);
	Delay_PID_ms(100, 1.2, 80);  // ����
	Pass_Short_d1d3();
	Front_Turn_Left();	
	Delay_PID_ms(300,3,20);
	Delay_PID_ms(200,2,50);
	
	
	//������ ��ת
	Delay_PID_ms(300, 1.2, 80);  // ����
	Beep(50,1);
	while(d1)           
	hunting_pid(3,40);
	while(d3)
	speed(18,18);
	speed(18,18);
	Delay_ms(200);
	stop();
	Delay_ms(50);
	Front_Turn_Left();	
	
	//�����������
	Delay_PID_ms(300,3,20);
	Beep(50,1);
	Delay_PID_ms(200,2,50);
	Beep(50,1);
	Delay_PID_ms(400,1.2,80);
	Beep(50,1);
	
	Part_3_78_L();
	
	//���������� ��ת ��һС��
	Front_Turn_Right();
	Pass_Short_d2d4();
	
	//��ת ������ ��ת
	while(d1)
	speed(35,-35);
	Delay_PID_ms(500,3,20);
	Delay_PID_ms(200,2,50);
	Beep(50,1);
	while(d2)
	hunting_pid(0.8,80);
	while(d4)
	hunting_pid(0.8,80);
	while(d2)
	hunting_pid(0.8,80);
	while(d4)
	hunting_pid(0.8,80);
	while(d2)
	hunting_pid(0.8,40);
	while(d4)
	speed(18,18);
	stop();
	Delay_ms(50);
	Front_Turn_Left();
	
	//׼���ؼ�
	Delay_PID_ms(300,3.5,25);
	Delay_PID_ms(200,2,50);
	Get_Home_1();
}



void go(void)   //��ʽ��������
{
	while(d0) stop();
	while(!d0) stop();
	First_Road();
	if(Color[1] == 2) One_Green();
	else if(Color[2] == 2) Two_Green();
	else if(Color[3] == 2) Three_Green();
	else if(Color[4] == 2) Four_Green();
	return ;
}

void go_test(u8 channel)  //openmv
{
	while(d0) stop();
	while(!d0) stop();
	First_Road_Test(channel);
	if(Color[1] == 2) One_Green();
	else if(Color[2] == 2) Two_Green();
	else if(Color[3] == 2) Three_Green();
	else if(Color[4] == 2) Four_Green();
	return ;
}
